#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#define fault 0.00001
#define PI 3.14159265358979323846
// c语言基于牛顿拉夫逊法的潮流计算

FILE *in;

// 定义支路参数结构体
typedef struct
{
    int from, to, status;
    double R, X, B;
    float k;

} branch;
// 定义发电机参数结构体
typedef struct
{
    int bus, status;
    float Pg, Qg, Vg;
} gen;

// baseMV:基准功率，buses:节点数，branches:支路数，PQs:平衡节点数，PVs:平衡节点数，blance_num:平衡节点编号,以0开始的，generators:发电机数, times:迭代次数,state:收敛状态
int baseMV = 100, buses, branches, PQs = 0, PVs = 0, blance_num, generators, times = 0, state = 1;

branch *Branch; // 所有支路的数据
gen *Gen;       // 所有发电机的数据

int *Bustype = 0;
double *P, *Q, *U0; // 节点功率和电压幅值
double *Udeg0;      // 电压相角
double **y_grid_a,
    **y_grid_b;                // 节点导纳矩阵，a=实部，b=虚部
double **jgrid;                // 雅可比矩阵
double *branch_p;              // 存放支路功率
double *branch_q;              // 存放支路功率
double *branch_Pi, *branch_Qi; // 存放i点支路注入功率
double *branch_Pj, *branch_Qj; // 存放j点支路注入功率
int *Pq_num, *Pv_num;          // 存放pq节点和pv节点的序号
int dim;                       // 雅可比矩阵的维数
double *dF;                    // 不平衡量
double *Hpp;                   // 求Hpp时累加用
float *BusGs, *BusBs;          // 节点的电导和电纳，a=实部，b=虚部
// double *branch_G, *branch_B;     // 节点的电导和电纳，a=实部，b=虚部
float *baseKV;      // 节点的基准电压
int *branchon;      // 存放支路是否可用，可用的话存放支路的序号，不可用的话存放0
int *gennum;        // 存放发电机的序号
int genones = 0;    // 发电机的个数
int branchones = 0; // 可用支路的个数
double *dx;         // 存放电压变化量
// float *PL;//用户的负载
// float *QL;//用户侧的无功
int *bushasgen; // 存放节点是否有发电机，有的话存放发电机的所在序号，没有的话存放0
float blance_GP = 0, blance_GQ = 0;
float *load_P, *load_Q; // 负载的有功和无功
float *busPVQ;          // Pv节点无功功率
//以优先列的形式输出稀疏矩阵格式
void showgrid(double **a, int n)
{

    int i, j;
    for (i = 0; i < dim; i++)
    {
        for (j = 0; j < dim; j++)
        {
            if (a[j][i])
            {
                printf("(%d,%d)  %8.6lf\n", j + 1, i + 1, a[j][i]);
            }
        }
    }
}
/// @brief 释放矩阵内存
void freegrid(double **a, int n)
{
    int i;
    for (i = 0; i < n; i++)
    {
        free(a[i]);
    }
    free(a);
}
/// @brief 显示复数矩阵
/// @param rm ：实部矩阵
/// @param im ：虚部矩阵
/// @param dim ：维度
void showgirdcomplex(double **rm, double **im, int dim)
{
    int i, j;
    // for (i = 0; i < dim; i++)
    // {
    //     for (j = 0; j < dim; j++)
    //     {
    //         if (rm[j][i] || im[j][i])
    //         {
    //             printf("(%d,%d)  %8.6lf+j%8.6lf\n", j + 1, i + 1, rm[i][j], im[i][j]);
    //         }
    //     }
    // }
    i = 41;
    for (j = 0; j < dim; j++)
    {
        if (rm[j][i] || im[j][i])
        {
            printf("(%d,%d)  %8.6lf+j%8.6lf\n", i + 1, j + 1, rm[i][j], im[i][j]);
        }
    }
}

double absf(double a)
{
    if (a < 0)
    {
        return -a;
    }
    return a;
}

/// @brief 读取matpower数据
void dataread(char *filename )
{
    int i;
    in = fopen(filename, "r");
    fscanf(in, "%d%d%d", &buses, &generators, &branches);
    // 分配内存
    Branch = (branch *)malloc(branches * sizeof(branch));
    Bustype = (int *)malloc(buses * sizeof(int));
    P = (double *)malloc(buses * sizeof(double));
    Q = (double *)malloc(buses * sizeof(double));
    U0 = (double *)malloc(buses * sizeof(double));
    Udeg0 = (double *)malloc(buses * sizeof(double));
    baseKV = (float *)malloc(buses * sizeof(float));
    Gen = (gen *)malloc(generators * sizeof(gen));
    BusGs = (float *)malloc(buses * sizeof(float));
    BusBs = (float *)malloc(buses * sizeof(float));
    bushasgen = (int *)malloc(buses * sizeof(int));
    load_P = (float *)malloc(buses * sizeof(float));
    load_Q = (float *)malloc(buses * sizeof(float));
    memset(bushasgen, 0, buses * sizeof(int));
    int num = 0; // 木桶排序序号，用来防止节点序号不是顺序
    float tmp;   // 临时存放数据，消除matlab的无关变量
    for (i = 0; i < buses; i++)
    {
        fscanf(in, "%d", &num);
        num--; // 从0开始存放节点序号
        fscanf(in, "%d%lf%lf%f%f%f%lf%lf%f%f%f%f", &Bustype[num], &P[num], &Q[num], &BusGs[num], &BusBs[num], &tmp,
               &U0[num], &Udeg0[num], &baseKV[num], &tmp, &tmp, &tmp);
        if (Bustype[num] == 1) // 统计pq和pv节点数
        {
            PQs++;
        }
        else if (Bustype[num] == 2)
        {
            PVs++;
        }
        else if (Bustype[num] == 3)
        {
            blance_num = num;
        }
        if (Udeg0[num])
        {
            Udeg0[num] *= PI / 180; // 转换为弧度制
        }
        load_P[num] = P[num];
        load_Q[num] = Q[num];
        P[num] /= (-baseMV); // 转换为标幺值,损耗转为注入功率
        Q[num] /= (-baseMV);
    }
    for (i = 0; i < generators; i++)
    {
        fscanf(in, "%d", &Gen[i].bus);
        Gen[i].bus--;
        fscanf(in, "%f%f", &Gen[i].Pg, &Gen[i].Qg);
        fscanf(in, "%f%f%f%f%d", &tmp, &tmp, &Gen[i].Vg, &tmp, &Gen[i].status);
        fscanf(in, "%f%f%f%f%f%f%f%f%f%f%f%f%f", &tmp, &tmp, &tmp, &tmp, &tmp, &tmp, &tmp, &tmp, &tmp, &tmp, &tmp, &tmp, &tmp);
    }

    for (i = 0; i < branches; i++)
    {
        fscanf(in, "%d%d%lf%lf%lf", &Branch[i].from, &Branch[i].to, &Branch[i].R, &Branch[i].X, &Branch[i].B);
        fscanf(in, "%f%f%f%f", &tmp, &tmp, &tmp, &Branch[i].k);
        fscanf(in, "%f%d%f%f", &tmp, &Branch[i].status, &tmp, &tmp);
        Branch[i].from--; // 从0开始存放节点序号
        Branch[i].to--;
    }
    fclose(in);
}

/// @brief 初始化数据，生成可用支路编号，生成pq和pv节点的编号，合并发电机和节点的数据生成P，Q，U；
void Init()
{

    dim = PQs * 2 + PVs; // 雅可比矩阵的维数
    int i, num = 0, n = 0;

    Pq_num = (int *)malloc(PQs * sizeof(int));
    Pv_num = (int *)malloc(PVs * sizeof(int));
    branchon = (int *)malloc(branches * sizeof(int));
    gennum = (int *)malloc(generators * sizeof(int));
    // 合并发电机和节点的数据生成P，Q，U；
    for (i = 0; i < generators; i++)
    {
        if (Gen[i].status == 1)
        {
            n = Gen[i].bus; // 临时变量，快速索引节点编号
            gennum[num] = i;
            P[n] += Gen[i].Pg / baseMV;
            Q[n] += Gen[i].Qg / baseMV;
            U0[n] = Gen[i].Vg;
            num++;
            bushasgen[n] = i + 1; // 记录有发电机节点的编号和对应发电机的位置
        }
    }
    genones = num;
    num = 0;
    n = 0;
    // 编写pq和pv节点的编号
    for (i = 0; i < buses; i++)
    {
        if (Bustype[i] == 1)
        {
            Pq_num[n] = i;
            n++;
        }
        else if (Bustype[i] == 2)
        {
            Pv_num[num] = i;
            num++;
        }
    }
    num = 0;
    // 记录可用支路编号
    for (i = 0; i < branches; i++)
    {
        if (Branch[i].status == 1)
        {
            branchon[num] = i;
            num++;
        }
    }
    branchones = num;

    jgrid = (double **)malloc((dim) * sizeof(double *));
    y_grid_a = (double **)malloc(buses * sizeof(double *));
    y_grid_b = (double **)malloc(buses * sizeof(double *));
    for (i = 0; i < buses; i++)
    {
        y_grid_a[i] = (double *)malloc(buses * sizeof(double));
        y_grid_b[i] = (double *)malloc(buses * sizeof(double));
    }
    // 初始化导纳矩阵
    for (i = 0; i < buses; i++)
    {
        memset(y_grid_a[i], 0, buses * sizeof(double));
        memset(y_grid_b[i], 0, buses * sizeof(double));
    }
    for (int i = 0; i < dim; i++)
    {
        jgrid[i] = (double *)malloc((dim) * sizeof(double));
        memset(jgrid[i], 0, dim * sizeof(double));
    }
}

/// @brief 生成节点导纳矩阵
void Getygrid()
{
    int i, n, m;

    // 计算节点导纳矩阵
    for (i = 0; i < branchones; i++)
    {
        int num_on = branchon[i];
        n = Branch[branchon[num_on]].from;
        m = Branch[num_on].to;
        double tmp = Branch[num_on].R * Branch[num_on].R + Branch[num_on].X * Branch[num_on].X;
        double tmpR, tmpX;
        if (Branch[num_on].k == 0)
        {
            tmpR = -Branch[num_on].R / (tmp);
            y_grid_a[n][m] += tmpR; // 公式推导，Z=R+jX，Y12=-1/Z
            y_grid_a[m][n] = y_grid_a[n][m];

            y_grid_a[n][n] -= tmpR;
            y_grid_a[m][m] -= tmpR;

            tmpX = Branch[num_on].X / (tmp);
            y_grid_b[n][m] += tmpX; // 与R符号相反
            y_grid_b[m][n] = y_grid_b[n][m];

            y_grid_b[n][n] -= tmpX;
            y_grid_b[m][m] -= tmpX;
            y_grid_b[n][n] += Branch[num_on].B / 2;
            y_grid_b[m][m] += Branch[num_on].B / 2;
        }
        else
        {
            tmpR = -Branch[num_on].R / (tmp) / Branch[num_on].k; // 防止两个节点有多条支路的情况
            y_grid_a[n][m] += tmpR;                              // 公式推导，Z=R+jX，Y12=-1/Z
            y_grid_a[m][n] = y_grid_a[n][m];

            y_grid_a[n][n] -= tmpR / Branch[num_on].k;
            y_grid_a[m][m] -= tmpR * Branch[num_on].k;

            tmpX = Branch[num_on].X / (tmp) / Branch[num_on].k;
            y_grid_b[n][m] += tmpX; // 与R符号相反
            y_grid_b[m][n] = y_grid_b[n][m];

            y_grid_b[n][n] -= tmpX / Branch[num_on].k; // Ynn
            y_grid_b[m][m] -= tmpX * Branch[num_on].k;
            y_grid_b[n][n] += Branch[num_on].B / 2;
            y_grid_b[m][m] += Branch[num_on].B / 2;
        }
    }
    for (i = 0; i < buses; i++)
    {
        if (BusBs[i] || BusGs[i]) // 将支路的电导电纳换算到实际标幺
        {
            BusBs[i] /= baseMV;
            BusGs[i] /= baseMV;
            y_grid_a[i][i] += BusGs[i];
            y_grid_b[i][i] += BusBs[i];
        }
    }

    // printf("y_grid_a:%d\n", times + 1);
    // showgirdcomplex(y_grid_a, y_grid_b, buses);
}

/// @brief 计算有功无功的不平衡量
void GetF()
{
    // m:pv节点序号数，n:pq节点序号数，k:雅可比矩阵的对应的pq节点序号数，l:雅可比矩阵的对应的pv节点序号数,z:节点序号
    int i, j;
    // for ( i = 0; i < buses; i++)
    // {
    //     printf("%f+i%f\n", P[i], Q[i]);
    // }

    int n = 0; // 存放实际的节点序号
    double tmp1 = 0, tmp2 = 0, sum1, sum2;
    dF = (double *)malloc(dim * sizeof(double));
    memset(dF, 0, dim * sizeof(double));
    Hpp = (double *)malloc(PVs * sizeof(double));
    for (i = 0; i < PQs; i++)
    {
        sum1 = 0;
        sum2 = 0;
        n = Pq_num[i];
        for (j = 0; j < buses; j++)
        {
            tmp1 = sin(Udeg0[n] - Udeg0[j]);
            tmp2 = cos(Udeg0[n] - Udeg0[j]);
            sum1 += U0[j] * (y_grid_a[n][j] * tmp2 + y_grid_b[n][j] * tmp1);
            sum2 += U0[j] * (y_grid_a[n][j] * tmp1 - y_grid_b[n][j] * tmp2);
        }
        dF[i * 2] = P[n] - sum1 * U0[n]; // pq节点的有功功率不平衡量

        dF[i * 2 + 1] = Q[n] - sum2 * U0[n]; // pq节点的无功功率不平衡量
    }
    for (i = 0; i < PVs; i++)
    {
        sum1 = 0;
        sum2 = 0;
        n = Pv_num[i];
        for (j = 0; j < buses; j++)
        {
            tmp1 = sin(Udeg0[n] - Udeg0[j]);
            tmp2 = cos(Udeg0[n] - Udeg0[j]);
            sum1 += U0[j] * (y_grid_a[n][j] * tmp2 + y_grid_b[n][j] * tmp1);
            sum2 += U0[j] * (y_grid_a[n][j] * tmp1 - y_grid_b[n][j] * tmp2);
        }
        dF[PQs * 2 + i] = P[n] - sum1 * U0[n];
        Hpp[i] = -sum2 * U0[n];
    }
    // 以matlab中的存储格式进行打印数据
    //  printf("F=\n");
    //  for ( i = 0; i < PVs; i++)
    //  {
    //      printf("%.12lf\n", dF[PQs*2+i]);
    //  }
    //  for ( i = 0; i < PQs; i++)
    //  {
    //      printf("%.12lf\n", dF[2* i]);
    //  }
    //  for ( i = 0; i < PQs; i++)
    //  {
    //      printf("%.12lf\n", dF[i * 2 + 1]);
    //  }
}

/// @brief 求雅可比矩阵
void GetJ()
{
    int i, j, n = 0, m = 0;
    double tmp1 = 0, tmp2 = 0;
    for (i = 0; i < PQs; i++)
    {
        n = Pq_num[i];
        for (j = 0; j < PQs; j++) // 遍历pq节点
        {
            m = Pq_num[j]; // 实际的pq节点序号
            tmp1 = sin(Udeg0[n] - Udeg0[m]);
            tmp2 = cos(Udeg0[n] - Udeg0[m]);
            if (i != j) // 非对角线元素
            {
                jgrid[i * 2][j * 2] = U0[n] * U0[m] * (y_grid_a[n][m] * tmp1 - y_grid_b[n][m] * tmp2);         // Hij
                jgrid[i * 2 + 1][j * 2] = -U0[n] * U0[m] * (y_grid_a[n][m] * tmp2 + y_grid_b[n][m] * tmp1);    // Jij
                jgrid[i * 2][j * 2 + 1] = U0[n] * U0[m] * (y_grid_a[n][m] * tmp2 + y_grid_b[n][m] * tmp1);     // Nij
                jgrid[i * 2 + 1][j * 2 + 1] = U0[n] * U0[m] * (y_grid_a[n][m] * tmp1 - y_grid_b[n][m] * tmp2); // Lij
            }
            else // 对角线元素
            {
                jgrid[i * 2][j * 2] = dF[i * 2 + 1] - Q[n] + U0[n] * U0[m] * (y_grid_a[n][m] * tmp1 - y_grid_b[n][m] * tmp2); // Hpp
                jgrid[i * 2 + 1][j * 2] = P[n] - dF[i * 2] - U0[n] * U0[m] * (y_grid_a[n][m] * tmp2 + y_grid_b[n][m] * tmp1); // Jpp
                jgrid[i * 2][j * 2 + 1] = jgrid[i * 2 + 1][j * 2] + pow(U0[n], 2) * 2 * y_grid_a[n][n];                       // Npp
                jgrid[i * 2 + 1][j * 2 + 1] = -jgrid[i * 2][j * 2] - pow(U0[n], 2) * 2 * y_grid_b[n][n];                      // Lpp
            }
        }
        for (j = 0; j < PVs; j++)
        {
            m = Pv_num[j];
            tmp1 = sin(Udeg0[n] - Udeg0[m]);
            tmp2 = cos(Udeg0[n] - Udeg0[m]);
            jgrid[i * 2][PQs * 2 + j] = U0[n] * U0[m] * (y_grid_a[n][m] * tmp1 - y_grid_b[n][m] * tmp2);      // Hpq
            jgrid[i * 2 + 1][PQs * 2 + j] = -U0[n] * U0[m] * (y_grid_a[n][m] * tmp2 + y_grid_b[n][m] * tmp1); // Jpq
        }
    }
    for (i = 0; i < PVs; i++)
    {
        n = Pv_num[i];
        for (j = 0; j < PQs; j++)
        {
            m = Pq_num[j];
            tmp1 = sin(Udeg0[n] - Udeg0[m]);
            tmp2 = cos(Udeg0[n] - Udeg0[m]);
            jgrid[PQs * 2 + i][j * 2] = U0[n] * U0[m] * (y_grid_a[n][m] * tmp1 - y_grid_b[n][m] * tmp2);     // Hpi
            jgrid[PQs * 2 + i][j * 2 + 1] = U0[n] * U0[m] * (y_grid_a[n][m] * tmp2 + y_grid_b[n][m] * tmp1); // Npi
        }
        for (j = 0; j < PVs; j++)
        {
            m = Pv_num[j];
            tmp1 = sin(Udeg0[n] - Udeg0[m]);
            tmp2 = cos(Udeg0[n] - Udeg0[m]);
            if (i != j)
            {
                jgrid[PQs * 2 + i][PQs * 2 + j] = U0[n] * U0[m] * (y_grid_a[n][m] * tmp1 - y_grid_b[n][m] * tmp2); // Hpp
            }
            else
            {
                jgrid[PQs * 2 + i][PQs * 2 + j] = Hpp[i] + U0[n] * U0[m] * (y_grid_a[n][m] * tmp1 - y_grid_b[n][m] * tmp2); // Hnp
            }
        }
    }
    free(Hpp);
//按照matlab的存储格式打印雅可比矩阵（废弃）
    // printf("jgrid:%d\n", times + 1);
    // double **p = (double **)malloc(dim * sizeof(double *));
    // for (i = 0; i < PVs; i++)
    // {
    //     p[i] = jgrid[PQs * 2 + i];//先是PV节点的有功不平衡量
    // }
    // for (i = 0; i < PQs; i++)//再是PQ节点的有功不平衡量
    // {
    //     p[PVs + i] = jgrid[i * 2];
    // }
    // for (i = 0; i < PQs; i++)
    // {
    //     p[dim - PQs + i] = jgrid[i * 2 + 1];
    // }
    // for (i = 0; i < dim; i++)
    // {
    //     for (j = 0; j < dim; j++)
    //     {
    //         if (p[j][i])
    //         {
    //             printf("(%d,%d)%lf\n", j + 1, i + 1, p[j][i]);
    //         }
    //     }
    // }
}

/// @brief 高斯消去法求解方程得电压变化量
void Get_dx()
{
    double tmp;
    double *tmpa;
    for (int i = 0; i < dim; i++)
    {

        tmp = jgrid[i][i]; // 防止主元为0
        if (tmp == 0)
        {
            for (int k = i + 1; k < dim; k++) // 如果主元为0，就与下面的行交换
            {

                if (jgrid[k][i] != 0)
                {
                    tmpa = jgrid[k];
                    jgrid[k] = jgrid[i];
                    jgrid[i] = tmpa;
                    tmp = dF[k];
                    dF[k] = dF[i];
                    dF[i] = tmp;
                    break;
                }
            }
            tmp = jgrid[i][i];
        }
        for (int j = 0; j < dim; j++) // 主元归一
        {
            jgrid[i][j] /= tmp;
        }
        dF[i] /= tmp;
        for (int j = 0; j < dim; j++) // 消元
        {
            if (j != i)
            {
                tmp = jgrid[j][i];
                for (int k = 0; k < dim; k++)
                {
                    jgrid[j][k] -= jgrid[i][k] * tmp;
                }
                dF[j] -= dF[i] * tmp;
            }
        }
    }
    dx = dF;
//    printf("ok");
}

/// @brief 解出计算节点电压变化量
void cal_U()
{
    // double *dx = (double *)malloc(dim * sizeof(double *));
    // memset(dx, 0, dim * sizeof(double *));
    // double *p, *q;
    // int i, j;
    // q = dx;
    // for (i = 0; i < dim; i++)
    // {
    //     p = jgrid_inv[i];
    //     for (j = 0; j < dim; j++)
    //     {
    //         *q += *p * dF[j];
    //         p++;
    //     }
    //     q++;
    // }
    double *q;
    int i;
    q = dx;
    state = 1;
    for (i = 0; i < dim; i++) // 判断是否收敛
    {
        if (absf(*q) > fault)
        {
            state = 0;
            break;
        }
        q++;
    }

    for (i = 0; i < PQs; i++)
    {
        Udeg0[Pq_num[i]] += dx[i * 2];
        U0[Pq_num[i]] += dx[i * 2 + 1] * U0[Pq_num[i]];
    }
    for (i = 0; i < PVs; i++)
    {
        Udeg0[Pv_num[i]] += dx[PQs * 2 + i];
    }
    // free(dx);
}

/// @brief 计算平衡节点功率和线路功率
void cal_PQ()
{
    freegrid(jgrid, dim);
    int i, in, to;
    double Uij, Uijdeg, tmp6, tmp7;
    double Ui2 = 0, Uj2 = 0;
    branch_p = (double *)malloc(branchones * sizeof(double));  // 损耗功率实部
    branch_q = (double *)malloc(branchones * sizeof(double));  // 损耗功率虚部
    branch_Pi = (double *)malloc(branchones * sizeof(double)); // 支路i端注入功率实部
    branch_Qi = (double *)malloc(branchones * sizeof(double)); // 支路i端注入功率虚部
    branch_Pj = (double *)malloc(branchones * sizeof(double)); // 支路j端注入功率实部
    branch_Qj = (double *)malloc(branchones * sizeof(double)); // 支路j端注入功率虚部
    busPVQ = (float *)malloc(buses * sizeof(float));
    memset(busPVQ, 0, buses * sizeof(float));

    double tmp1 = 0, tmp2 = 0;
    for (i = 0; i < buses; i++)
    {
        tmp1 += y_grid_a[blance_num][i] * U0[i] * cos(Udeg0[i]) - y_grid_b[blance_num][i] * U0[i] * sin(Udeg0[i]);
        tmp2 += y_grid_a[blance_num][i] * U0[i] * sin(Udeg0[i]) + y_grid_b[blance_num][i] * U0[i] * cos(Udeg0[i]);
    }
    tmp2 = -tmp2;
    blance_GP = U0[blance_num] * cos(Udeg0[blance_num]) * tmp1 - U0[blance_num] * sin(Udeg0[blance_num]) * tmp2; // 平衡节点注入功率实部
    blance_GQ = U0[blance_num] * sin(Udeg0[blance_num]) * tmp1 + U0[blance_num] * cos(Udeg0[blance_num]) * tmp2; // 平衡节点注入功率虚部
    float branchG1, branchB1;                                                                                    // i节点的电导和电纳
    float branchG2, branchB2;                                                                                    // j节点的电导和电纳
    int num_on;
    for (i = 0; i < branchones; i++) // 计算支路的ij注入功率
    {
        num_on = branchon[i];
        in = Branch[num_on].from;
        to = Branch[num_on].to;
        // 相量Ui*共轭Uj
        Uij = U0[in] * U0[to];
        Uijdeg = Udeg0[in] - Udeg0[to];
        if (Branch[i].k)
        {
            branchG1 = y_grid_a[in][to] / Branch[num_on].k * (Branch[num_on].k - 1); // 变压器换算后的对地电导
            branchB1 = y_grid_b[in][to] / Branch[num_on].k * (Branch[num_on].k - 1);
            branchB2 = y_grid_a[in][to] * (1 - Branch[num_on].k);
            branchG2 = y_grid_a[in][to] * (1 - Branch[num_on].k);
            branchB1 += Branch[num_on].B / 2;
            branchB2 += Branch[num_on].B / 2;
        }
        else
        {
            branchG1 = BusGs[in];            // 支路i端对地电导
            branchB1 = Branch[num_on].B / 2; // 支路i端对地电纳
            branchG2 = BusGs[to];
            branchB2 = Branch[num_on].B / 2;
        }

        Ui2 = pow(U0[in], 2); // 中间量
        Uj2 = pow(U0[to], 2);
        tmp6 = Uij * cos(Uijdeg) * y_grid_a[in][to] + Uij * sin(Uijdeg) * y_grid_b[in][to]; // 后面两项的实部，ij
        tmp7 = Uij * sin(Uijdeg) * y_grid_a[in][to] - Uij * cos(Uijdeg) * y_grid_b[in][to]; // 后面两项的虚部，ij

        branch_Pi[i] = Ui2 * (branchG1 + y_grid_a[in][to]) - tmp6; // 支路i端注入功率实部
        branch_Qi[i] = Ui2 * (branchB1 - y_grid_b[in][to]) - tmp7; // 支路j端注入功率虚部

        Uijdeg = Udeg0[to] - Udeg0[in];                                                     // 相量Uj*共轭Ui相角差
        tmp6 = Uij * cos(Uijdeg) * y_grid_a[to][in] + Uij * sin(Uijdeg) * y_grid_b[to][in]; // 后面两项的实部，ji
        tmp7 = Uij * sin(Uijdeg) * y_grid_a[to][in] - Uij * cos(Uijdeg) * y_grid_b[to][in]; // 后面两项的虚部，ji
        branch_Pj[i] = Uj2 * (branchG2 + y_grid_a[to][in]) - tmp6;                          // 支路功率实部
        branch_Qj[i] = Uj2 * (branchB2 - y_grid_b[to][in]) - tmp7;                          // 支路功率虚部

        if (Bustype[in] == 2)
        {
            busPVQ[in] += branch_Qi[i];
        }
        if (Bustype[to] == 2)
        {
            busPVQ[to] += branch_Qj[i];
        }

        branch_p[i] = branch_Pi[i] + branch_Pj[i] - Ui2 * branchG1 - Uj2 * branchG2; // 支路损耗功率实部
        branch_q[i] = branch_Qi[i] + branch_Qj[i] - Ui2 * branchB1 - Uj2 * branchB2; // 支路损耗功率虚部
    }
}

/// @brief 打印输出结果
void print()
{
    int i;
    float alllossP = 0, alllossQ = 0;

    for (i = 0; i < branchones; i++)
    {
        alllossP += branch_p[i];
        alllossQ += branch_q[i];
    }

    printf("%%================================================================================================\n");
    printf("= \n%80s\n", "系统总结");
    printf("%%================================================================================================\n");
    printf("迭代了%d次\n", times);
    printf("发电机数目：%-6d PQ节点数目:%-6d  PV节点数目:%-6d\n", genones, PQs, PVs);
    printf("平衡节点注入功率: %5.2f+j%-5.2f\n", blance_GP * baseMV, blance_GQ * baseMV);
    printf("总线路损耗功率: %5.2f+j%-5.2f\n", -alllossP * baseMV, -alllossQ * baseMV);
    printf("\n");
    printf("%%================================================================================================\n");
    printf("= \n%90s\n", "节点数据");
    printf("%%================================================================================================\n");
    printf("%6s%6s%18s%20s%20s%20s\n", "Bus", "type", "Mag(pu)", "Ang(deg)", "GenS(MW)", "LoadS(MW)");
    printf("%6s%6s%18s%20s%20s%20s\n", "----", "----", "-----------------", "-----------------", "-----------------", "-----------------");
    for (i = 0; i < buses; i++)
    {

        printf("%6d%6d%18.3f%20.3f ", i + 1, Bustype[i], U0[i], Udeg0[i] * 180 / PI); // 先打印节点编号，节点类型，电压幅值，电压相角
        if (bushasgen[i])                                                             // 假如有发电机
        {
            if (Bustype[i] == 2)
            {
                printf("%10.2f+j%-10.2f", P[i] * baseMV + load_P[i], -busPVQ[i] * baseMV + load_Q[i]); // P+L=G,负载的注入功率为负
            }
            else if (Bustype[i] == 1)
            {
                printf("%10.2f+j%-10.2f", P[i] * baseMV + load_P[i], Q[i] * baseMV + load_Q[i]);
            }
            else if (Bustype[i] == 3)
            {
                printf("%10.2f+j%-10.2f", load_P[i] + blance_GP * baseMV, load_Q[i] + blance_GQ * baseMV);
            }
        }
        else
        {
            printf("%11s%-11s", "-", "-");
        }
        printf("%10.2f+j%-10.2f\n", load_P[i], load_Q[i]);
    }

    printf("%%================================================================================================\n");
    printf("= \n%90s\n", "支路数据");
    printf("%%================================================================================================\n");
    printf("%6s%6s%6s%24s%24s%20s\n", "Branch", "From", "to", "From Bus Injection", "To Bus Injection", "Loss(I^2*Z)");
    printf("%-8s%6s%12s%12s%12s%12s%12s%12s\n", "#    Bus", "Bus", "P(MW)", "Q(MVAr)", "P(MW)", "Q(MVAr)", "P(MW)", "Q(MVAr)");

    printf("%-6s%6s%6s%12s%12s%12s%12s%12s%12s\n", "---", "----", "-----", "-------", "---------", "---------", "---------", "---------", "---------");
    for (i = 0; i < branches; i++)
    {
        printf("%-6d%6d%6d%12.2lf%12.2lf%12.2lf%12.2lf%12.3lf%12.2lf\n", i + 1,
               Branch[i].from + 1, Branch[i].to + 1, -branch_Pi[i] * baseMV, -branch_Qi[i] * baseMV, -branch_Pj[i] * baseMV,
               -branch_Qj[i] * baseMV, -branch_p[i] * baseMV, -branch_q[i] * baseMV); // 支路编号，支路起始节点，支路终止节点，支路起始节点注入功率，支路终止节点注入功率，支路损耗功率
    }
    printf("END\n");
}

/// @brief 释放内存
void free_memory()
{
    for (int i = 0; i < PQs * 2 + PVs; i++)
    {
        free(jgrid[i]);
    }
    free(jgrid);
    free(Bustype);
    free(P);
    free(Q);
    free(U0);
    free(Udeg0);
    free(branch_p);
    free(branch_q);
    free(branch_Pi);
    free(branch_Qi);
    free(branch_Pj);
    free(branch_Qj);
    free(Gen);
    free(Branch);
    free(Pq_num);
    free(Pv_num);
    free(dF);
    free(baseKV);
    free(branchon);
    free(gennum);
    free(Branch);
    free(bushasgen);
    free(busPVQ);
}

int main()
{
    char *filename = "./data/data4.txt";
    dataread(filename); // 读取数据
    Init();     // 初始化数据，分配内存
    Getygrid(); // 生成节点导纳矩阵
    while (1)
    {

        GetF();   // 计算不平衡量
        GetJ();   // 计算雅可比矩阵
        Get_dx(); // 计算电压变化量
        cal_U();  // 计算电压的修正量
        times++;  // 迭代次数加1
        //printf("第%d迭代已完成\n", times);
        if (state == 1)
        {
            cal_PQ(); // 结果收敛，计算线损和平衡节点功率
            print();  // 打印输出结果
            free_memory();
            break;
        }
        if (times > 12)
        {
            printf("收敛失败\n");
            break;
        }
    }
    
    return 0;
    
}
